#include <math.h>
#include <stdlib.h>
#include <stdio.h>
double G;	
double eps = .1;
int iterations;
int bodies;
double dt;
double *mass,*vel,*acc,*pos;

double norm2(double val[]) {
    double result = 0;
    for (int i = 0; i < 3; i++)
        result += pow(val[i], 2);
    return result;
}

double * getPos(int body, int step) {
    static double a[3];
    int mult = iterations * bodies;
    int index = body + bodies * step;
    for (int i = 0; i < 3; i++) {
        a[i] = pos[index + mult * i];
    }
    return a;
}

void getPos2(double *temp, int body, int step) {
    int mult = iterations * bodies;
    int index = body + bodies * step;
    for (int i = 0; i < 3; i++) {
        temp[i] = pos[index + mult * i];
    }
}

void print(double p[]) {
    printf("%e,%e,%e\n", p[0], p[1], p[2]);
}

void setPos(double x, double y, double z, int body, int currentStep) {   
    int mult = iterations * bodies;
    int index = body + bodies * currentStep;
    pos[index] = x;
    pos[index + mult] = y;
    pos[index + mult * 2] = z;            
}

void setPos2(double res[], int body , int currentStep) {
    int mult = iterations * bodies;
    int index = body + bodies * currentStep;
    for (int i = 0; i < 3; i++)
        pos[index + mult * i] = res[i];
}

void setVelocity(double res[], int body) {
    int mult = bodies;
    int index = body;
    for (int i = 0; i < 3; i++)
        vel[index + mult * i] = res[i];
}

void getVelocity(double res[], int body) {
    for (int i = 0; i < 3; i++)
        res[i] = vel[body + bodies * i];
}

void setAcc(double res[], int body) {
    int mult = bodies;
    int index = body;
    for (int i = 0; i < 3; i++)
        acc[index + bodies * i] = res[i];
}

void getAcc(double res[], int body) {
    int mult = bodies;
    int index = body;
    for (int i = 0; i < 3; i++)
        res[i] = acc[index + mult * i];
}

/*Subtract b from a*/
void diff(double res[], double a[], double b[]) {
    for (int i = 0; i < 3; i++)
        res[i] = a[i] - b[i];

}

void add(double res[], double a[], double b[]) {
    for (int i = 0; i < 3; i++)
        res[i] = a[i] + b[i];
}

void mult(double res[], double a[], double c) {
    for (int i = 0; i < 3; i++)
        res[i] = a[i] * c;
}

void nextAcc(double *next, int body, int step) {
    for (int i = 0; i < bodies; i++) {
        if (i != body) {
            double radius[3];
            double posa[3];
            double posb[3];
            getPos2(posa, i, step);
            getPos2(posb, body, step);
            diff(radius, posa, posb);
            double div = pow(norm2(radius) + eps, 1.5);
            mult(radius, radius, mass[i] / div);
            add(next, next, radius);
        }
    }
    mult(next, next, G);
}

void updateVel(int body, int step) {
    double next[3];
    double temp[3];
    nextAcc(next, body, step);
    double tacc[3];
    getAcc(tacc, body);
    add(temp, next, tacc);
    mult(temp, temp, .5 * dt);
    double vel2[3];
    getVelocity(vel2, body);
    add(vel2, vel2, temp);
    setVelocity(vel2, body);
    setAcc(next, body);
}

void update(int body, int step) {    
    double temp[3];
    double tvel[3];
    updateVel(body, step);
    getPos2(temp, body, step);
    getVelocity(tvel, body);
    mult(tvel, tvel, dt);
    add(temp, tvel, temp);
    if(step + 1 < iterations)
        setPos2(temp, body, step + 1);
}

void set(double timestep, int totalBodies, int l) {
    G = 6.673 * pow(10, -11);
    bodies = totalBodies;
    dt = timestep;
    iterations = l;
}

void setVar(double *tmass, double *tacc, double *tvel, double *tpos) {   
    mass = tmass;    
    acc = tacc;
    vel = tvel;
    pos = tpos;
}
